//Project 3 - CS 3451 - Labyrinth Lite
//Instructor: Jarek Rossignac
//Author: Tyler Powell
//Date: September 2011

float gravitationalConstant = 1;
vec currentVelocity = new vec(0,0);

void incrementGConst(int i) {
  gravitationalConstant+=i;
}

void incrementGConst() {
  incrementGConst(1);
}

void decrementGConst(int i) {
  gravitationalConstant-=i;
}

void decrementGConst() {
  decrementGConst(1);
}

vec acceleration() { //acceleration of ball based on displacement of mouse from center of screen
  pt centerPt = new pt(width/2,height/2);
  pt mousePt = new pt(mouseX,mouseY);
  vec a = new vec(centerPt,mousePt);
  a.scaleBy(gravitationalConstant);
  return a;
}

vec velocity(float time, vec initialVelocity) { //determine the velocity of the ball at time t given its initial velocity
  vec v = new vec();
  v.x = initialVelocity.x + time * acceleration().x;
  v.y = initialVelocity.y + time * acceleration().y;
  return v;
}

void updateVelocity(float time, vec initialVelocity){
  currentVelocity = velocity(time, initialVelocity);
}

pt location(float time, vec initialVelocity, pt initialLocation) { //determine the location of the ball at time t given initial conditions
  pt finalLocation = new pt();
  finalLocation.x = initialLocation.x + initialVelocity.x*time + 0.5*acceleration().x*pow(time,2);
  finalLocation.y = initialLocation.y + initialVelocity.y*time + 0.5*acceleration().y*pow(time,2);
  return finalLocation;
}

float timeAt(pt p, vec initialVelocity, pt initialLocation){ //determine the time at which the ball locations is a point p
  vec accel = acceleration();
  float velNorm = initialVelocity.norm();
  float accelNorm = accel.norm();
  vec uvel = U(initialVelocity);
  vec uaccel = U(accel);
  float addNorm = uvel.add(uaccel).norm();
  if(addNorm < sqrt(2.0)){
      accelNorm=-accelNorm;
  }
  float ix = (-initialVelocity.x + sqrt(sq(initialVelocity.x) - (2*accel.x*(p.x-initialLocation.x))))/accel.x;
  float iy = (-initialVelocity.y + sqrt(sq(initialVelocity.y) - (2*accel.y*(p.y-initialLocation.y))))/accel.y;
  float jx = (-initialVelocity.x - sqrt(sq(initialVelocity.x) - (2*accel.x*(p.x-initialLocation.x))))/accel.x;
  float jy = (-initialVelocity.y - sqrt(sq(initialVelocity.y) - (2*accel.y*(p.y-initialLocation.y))))/accel.y;
  vec iv = new vec(ix,iy);
  vec jv = new vec(jx,jy);
  float i = iv.norm();
  float j = jv.norm();
//  float i = (-velNorm + sqrt(sq(velNorm) - (4*0.5*accelNorm*d(p,initialLocation))))/accelNorm;
//  float j = (-velNorm - sqrt(sq(velNorm) - (4*0.5*accelNorm*d(p,initialLocation))))/accelNorm;
//  float i = d(initialLocation,p)/velNorm;
//  println(i + " " + j); 
  return min(i,j)/FPS;
//  println(i);
//  return i + time;
}

float newTimeAt(pt p, vec initialVelocity, pt initialLocation){
  vec accel = acceleration();
  float velNorm = initialVelocity.norm();
  float accelNorm = accel.norm();
  vec uvel = U(initialVelocity);
  vec uaccel = U(accel);
  float addNorm = uvel.add(uaccel).norm();
  if(addNorm < sqrt(2.0)){
      accelNorm=-accelNorm;
  }
  vec pd = new vec(initialLocation,p);
  float i = -(-(2*dot(pd,initialVelocity)) + sqrt(sq(2*dot(pd,initialVelocity)) - 4*(dot(initialVelocity,initialVelocity)*(dot(pd,pd)-sq(ballRadius)))))/(2*dot(initialVelocity,initialVelocity));
  float j = -(-(2*dot(pd,initialVelocity)) - sqrt(sq(2*dot(pd,initialVelocity)) - 4*(dot(initialVelocity,initialVelocity)*(dot(pd,pd)-sq(ballRadius)))))/(2*dot(initialVelocity,initialVelocity));
//  float i = (-velNorm + sqrt(sq(velNorm) - (4*0.5*accelNorm*d(p,initialLocation))))/accelNorm;
//  float j = (-velNorm - sqrt(sq(velNorm) - (4*0.5*accelNorm*d(p,initialLocation))))/accelNorm;
//  float i = d(initialLocation,p)/velNorm;
  println(i + " " + j);
  if(i>0){
    return i;
  } else {
    return j;
  }
//  return min(i,j)/FPS;
//  println(i);
//  return i + time;
}

float newTimeAt(pt p, float t) {
  pt a = location(time + t, currentVelocity,ballLocation);
  println(a.x + " " + a.y);
  println(p.x + " " + p.y);
  
  if(p.x + ballRadius > a.x && p.x-ballRadius < a.x && p.y + ballRadius > a.y && p.y-ballRadius<a.y){
    return t;
  } else if(d(ballLocation,p) < d(ballLocation,a)){
    return newTimeAt(p, t/2);
  } else {
    return newTimeAt(p, t+t/2);
  }
}
